﻿// FLMScanFactory
// Object representing Riedmaier reconstruction algorithm
// 
// FLMScan - Featured by FabLab Munich

using System;
using FLMScan.Geometry;


namespace FLMScan.WorldCoordinateReconstruction.Riedmaier
{
    class RiedmaierReconstruction : IReconstruction
    {
        private CameraSensor cameraSensor;
        private LaserPlane laserPlane;
        private RotationAxis rotationAxis;

        RiedmaierWorldCoordinateReconstruction reconstruction;

        public RiedmaierReconstruction(CameraSensor cameraSensor, LaserPlane laserPlane, RotationAxis rotationAxis)
        {
            this.cameraSensor = cameraSensor;
            this.laserPlane = laserPlane;
            this.rotationAxis = rotationAxis;

            reconstruction = new RiedmaierWorldCoordinateReconstruction(laserPlane, cameraSensor);
        }


        FlmPoint IReconstruction.reconstruct(FlmPoint imagePlanePoint, double angleInRadians)
        {
            FlmPoint relativePoint = new FlmPoint(imagePlanePoint.X - cameraSensor.PrimaryPointInPixelsX, imagePlanePoint.Y - cameraSensor.PrimaryPointInPixelsY);

            FlmPoint imagePointInMillimeters =
                new FlmPoint(cameraSensor.ConvertPixelsToMillimetersX(relativePoint.X), cameraSensor.ConvertPixelsToMillimetersY(relativePoint.Y));

            FlmPoint reconstructedPoint = reconstruction.reconstruct(imagePointInMillimeters);

            double pointDistance = rotationAxis.GetPointDistance(reconstructedPoint);
            double worldPointX = pointDistance * Math.Cos(angleInRadians);
            double worldPointZ = reconstructedPoint.Y;
            double worldPointY = pointDistance * Math.Sin(angleInRadians);

            FlmPoint worldPoint = new FlmPoint(worldPointX, worldPointY, worldPointZ);
            
            return worldPoint;
        }
    }
}
